#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "hi_pwm.h"
#include "hi_timer.h"
#include "iot_pwm.h"

//左右两轮电机各由一个L9110S驱动
//GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车。
#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

//查阅机器人板原理图可知
//左边的红外传感器通过GPIO11与3861芯片连接
//右边的红外传感器通过GPIO12与3861芯片连接
#define GPIO11 11
#define GPIO12 12

#define GPIO_FUNC 0
#define car_speed_left 0
#define car_speed_right 0
extern unsigned char g_car_status;   

unsigned int g_car_speed_left = car_speed_left;
unsigned int g_car_speed_right = car_speed_right;
IotGpioValue io_status_left;
IotGpioValue io_status_right;

//获取红外传感器的值，调整电机的状态
void timer1_callback(unsigned int arg)
{
    IotGpioValue io_status;

    //调整左轮状态 
    //如果左轮处于停止状态（GPIO1为高电平），则左边红外传感器没检测黑色的话把左轮处于正转状态（GPIO1为低电平）
    if(g_car_speed_left != car_speed_left)
    {
        IoTGpioGetInputVal(GPIO11,&io_status);
        if(io_status != IOT_GPIO_VALUE0){
            g_car_speed_left = car_speed_left;
            printf("left speed change \r\n");
        }
    }

    //调整右轮状态
    //如果右轮处于停止状态（GPIO10为高电平），则右边红外传感器没检测黑色的话把右轮处于正转状态（GPIO10为低电平）
    if(g_car_speed_right != car_speed_right)   
    {
        IoTGpioGetInputVal(GPIO12,&io_status);
        if(io_status != IOT_GPIO_VALUE0){
            g_car_speed_right = car_speed_right;
            printf("right speed change \r\n");
        }
    }
    
    //小车处于停止状态，则把小车变成前进状态（如果GPIO1和GPIO10输出高电平，则GPIO1和GPIO10输出低电平）
    if(g_car_speed_left != car_speed_left && g_car_speed_right != car_speed_right)
    {
        g_car_speed_left = car_speed_left;
        g_car_speed_right = car_speed_right;
    }

    IoTGpioGetInputVal(GPIO11,&io_status_left); //获取GPIO11引脚的输入电平值
    IoTGpioGetInputVal(GPIO12,&io_status_right);//获取GPIO12引脚的输入电平值
    
    //小车往左偏，则需要向右修正方向(右转)
    //如果GPIO12输入低电平（右边的红外传感器检测到黑色）并且GPIO11输入高电平（左边的红外传感器未检测到黑色）
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0)
    {   
        //则GPIO1输出低电平,GPIO10输出高电平。小车右转（右轮不转，左轮正转）
        g_car_speed_left = car_speed_left;
        g_car_speed_right = 1;
    } 

    //小车往右偏，则需要向左修正方向（左转）
    //如果GPIO12输入高电平（右边的红外传感器未检测到黑色）并且GPIO11输入低电平（左边的红外传感器检测到黑色）
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        //则GPIO1输出高电平,GPIO10输出低电平。小车左转（右轮正转，左轮不转）
        g_car_speed_left = 1;
        g_car_speed_right = car_speed_right;
    }

}

void trace_module(void)
{  
    unsigned int timer_id1;
    hi_timer_create(&timer_id1);
    
    //启动系统周期定时器用来按照预定的时间间隔1ms触发timer1_callback任务的执行
    hi_timer_start(timer_id1, HI_TIMER_TYPE_PERIOD, 1, timer1_callback, 0);

    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, g_car_speed_left);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, g_car_speed_right);
    
    while (1) { 
        gpio_control(GPIO1, g_car_speed_left);
        gpio_control(GPIO10, g_car_speed_right);

        hi_udelay(20);

        if (g_car_status != CAR_TRACE_STATUS) {
            break;
        }
    }
    hi_timer_delete(timer_id1);
}